using UnityEngine;
using System.Collections;
using System.Collections.Generic;


public class lwAstar_Heap : ArrayList
{
	public void Push(object obj)
	{
	    Insert(0, obj);
	}

	public object Pop()
	{
		object obj = this[0];
		RemoveAt(0);
		return obj;
	}

	public lwAstarNode PopClosest()
	{
		float fMinCost = Mathf.Infinity;
		int nMinIndex = -1;
		for (int i = 0; i < Count; i++)
		{
			lwAstarNode test = this[i] as lwAstarNode;
			if (test.TotalEstimatedCost < fMinCost)
			{
				fMinCost = test.TotalEstimatedCost;
				nMinIndex = i;
			}
		}
		if (nMinIndex == -1)
		{
			Debug.LogError("PopClosest");
			return null;
		}
		lwAstarNode res = this[nMinIndex] as lwAstarNode;
		RemoveAt(nMinIndex);
		return res;
	}
}


public sealed class lwAstarPathFinder
{
	public List<lwAstarNode> m_AllNodes;

	private lwAstar_Heap m_openList = new lwAstar_Heap();
	private lwAstar_Heap m_closedList = new lwAstar_Heap();



	public lwAstarNode[] FindPath(lwAstarNode startNode, lwAstarNode goalNode)
	{
		if( !m_AllNodes.Contains(startNode) || !m_AllNodes.Contains(goalNode) )
			Debug.LogError("Where does those nodes comes from !!!");
	
		m_openList.Clear();
		m_closedList.Clear();

		startNode.Parent = null;
		startNode.WeightedGlobalCost = 0;
		startNode.GlobalCost = 0;
		m_openList.Push(startNode);

		int nNodeCount = m_AllNodes.Count;

		for (int i = 0; m_openList.Count > 0 && i < nNodeCount; i++)
		{
			lwAstarNode CurrentNode = m_openList.PopClosest();
			m_closedList.Push(CurrentNode);
			if (CurrentNode == goalNode)
			{
				//Goal reached
				List<lwAstarNode> Solution = new List<lwAstarNode>();

				while (CurrentNode != null)
				{
					Solution.Insert(0, CurrentNode);
					CurrentNode = CurrentNode.Parent;
				}
				return Solution.ToArray();
			}
			else
			{
				//Searching
				//CurrentNode.PrintNodeInfo();
				
				foreach(lwAstarNode.lwAstarLink outlink in CurrentNode.m_Links)
				{
					//outlink.dest.PrintNodeInfo();
					if (!m_closedList.Contains(outlink.dest))
					{
						if (outlink.dest.blocked && outlink.dest != goalNode )
						{
							m_closedList.Add(outlink.dest);
						}
						else
						{
							float currentWeightedGlobalCost = CurrentNode.WeightedGlobalCost + ( outlink.cost * outlink.costMultiplier + outlink.weight * outlink.weightMultiplier );
							float currentGlobalCost = CurrentNode.GlobalCost + outlink.cost;
							if (!m_openList.Contains(outlink.dest))
							{
								m_openList.Push(outlink.dest);
								outlink.dest.Parent = CurrentNode;
								outlink.dest.WeightedGlobalCost = currentWeightedGlobalCost;
								outlink.dest.GlobalCost = currentGlobalCost;
								outlink.dest.RemainingHeuristicCost = lwAstarNode.ComputeHeuristic(outlink.dest, goalNode);
								//Debug.Log("new node added");
							}
							else if( currentWeightedGlobalCost < outlink.dest.WeightedGlobalCost )
							{
								outlink.dest.Parent = CurrentNode;
								outlink.dest.WeightedGlobalCost = currentWeightedGlobalCost;
								outlink.dest.GlobalCost = currentGlobalCost;
								outlink.dest.RemainingHeuristicCost = lwAstarNode.ComputeHeuristic(outlink.dest, goalNode);
								//Debug.Log("node updated with better path");
							}
						}
					}
					else
					{
						//Debug.Log("Already in closed list");	
					}

				}

			}
	
		}
		Debug.Log("No Path Found ... returning null");	
		return null;
	}
}
